# -*- coding: utf-8 -*-
"""
Created on Mon Mar 28 21:51:14 2022

@author: lizhuang
"""
import numpy as np
def R6dy(x):
    #function y = R6dy(x) 
    # 
    # Dynamic analysis for RRRRRR 3 degree bar group 
    # 
    # Copyrigl1t 2003 Qu Xiuquan 
    # 
    # Input parameters 
    # 
    # x(l) = theta - i 
    # x(2) = theta - j 
    # x(3) = theta - k 
    # x(4) = theta
    # x(5) = theta - f 
    # x(6) = theta - g 
    # x(7) = dtheta
    # x(8) = dtheta
    # x(9) = dtheta - k 
    # x(10) = dtheta - g 
    # x(11) = ddtheta
    # x(l2) = ddtheta
    # x(l3) = ddtheta - k 
    # x(l4) = ddtheta - g
    # x(15) = Re[ddB] 
    # x(l6) = Im[ddB] 
    # x(17) = Re[ddE] 
    # x(18) = Im[ddE] 
    # 
    # Output parameters 
    # 
    # y(l) = RxB 
    # y(2) = RyB 
    # y(3) = RxC 
    # y(4) = Rye 
    # y(5) = RxD 
    # y(6) = RyD 
    # y(7) = RxE 
    # y(8) = RyE 
    # y(9) = RxF 
    # y(lO) = RyF 
    # y(ll) = RxG 
    # y(12) = RyG 
    # 
    pi=np.pi;cos=np.cos;sin=np.sin
    g=9.8; 
    ri=0.4;rj = 0.3;rk = 0.3;rci = 0.2;rcj = 0.15;rck = 0.15;rce = 0.274;rcf= 0.188;rcg = 0.095; 
    thgp = 0.5612;thep = 0.1931;thfp = 0.5214; 
    mi= 8.5;mj = 7.6;mk = 7.6;ml = 12; 
    Ji= 0.09;Jj=0.075;Jk = 0.075;Jl = 0.15; 
    ReddC = 0;ImddC = 0;ReddD = 0;ImddD = 0; 
    Fxi = 0;Fyi = 0;Fxj = 0;Fyj = 0;Fxk = 0;Fyk = 0;Fxl = 0;Fyl = 0; 
    Mi = 0;Mj = 1000;Mk = 1000;Ml = 0; 
    a = np.zeros((12,12)); 
    a[0,0]=1;a[0,6]=1; 
    a[1,1]=1;a[1,7]= 1; 
    a[2,0] = rci * sin(x[0]);a[2,1] = - rci* cos(x[0]);a[2,6] = - (ri- rci) * sin(x[0]); 
    a[2,7] = (ri-rci) * cos(x[0]) ; 
    a[3,2] = 1;a[3,8] = 1; 
    a[4,3] = 1;a[4,9] = 1; 
    a[5,2] = rcj * sin(x[1]) ;a[5,3] = - rcj * cos(x[1]) ;a[5,8] = - (rj - rcj) * sin(x[1]); 
    a[5,9] = (rj - rcj) * cos(x[1]); 
    a[6,4] = 1;a[6,10] = 1; 
    a[7,5] = 1;a[7,11] = 1; 
    a[8,4] = rck * sin(x[2]) ;a[8,5] = - rck * cos(x[2]) ;a[8,10]= - (rk - rck) * sin(x[2]); 
    a[8,11] = (rk - rck) * cos(x[2]); 
    a[9,6]=-1;a[9,8] =-1;a[9,10] =-1; 
    a[10,7] =- 1;a[10,9] =- 1;a[10,11] =-1; 
    a[11,6] = -rcg*sin(x[5]+thgp);a[11,7] = rcg*cos(x[5]+thgp); 
    a[11,8] = - rce * sin(x[3] + thep) ;a[11,9] = rce * cos(x[3] + thep); 
    a[11,10] = -rcf*sin(x[4] + thfp) ;a[11,11] =rcf* cos(x[4] + thfp); 
    b = np.zeros((12,1)); 
    b[0,0] = mi*x[14] +mi*rci*x[10]*cos(x[0] +pi/2) +mi* rci * x[6]**2 * cos(x[0] + pi) - Fxi; 
    b[1,0] = mi* x[15] +mi* rci * x[10] * sin(x[0] + pi/2) + mi*rci*x[6]**2*sin(x[0] + pi)-Fyi + mi* g; 
    b[2,0]=Ji*x[10]-Mi
    b[3,0]= mj * ReddC + mj * rcj * x[11] * cos(x[1] + pi/2) + mj * rcj * x[7]**2 * cos(x[1] + pi) - Fxj; 
    b[4,0]= mj * ImddC + mj * rcj * x[11] * sin(x[1] + pi/2) + mj * rcj * x[7]**2 * sin(x[1] + pi) -Fyj + mj * g; 
    b[5,0] =Jj*x[11]-Mj; 
    b[6,0] = mk * ReddD + mk * rck * x[12] * cos(x[2] + pi/2) + mk * rck * x[8]**2 * cos(x[2] + pi) - Fxk; 
    b[7,0] = mk * ImddD+ mk * rck * x[12] * sin(x[2] + pi/2) + mk * rck * x[8]**2 * sin(x[2] + pi) -Fyk + mk* g; 
    b[8,0] = Jk * x[12] - Mk; 
    b[9,0]=ml*x[16]+ml*rcg*x[13]*cos(x[5]+thgp+pi/2)+ml*rcg*x[9]**2*cos(x[5]+thgp+pi)-Fxl
    b[10,0] = ml*x[17] + ml* rcg* x[13] * sin(x[5] + thgp + pi/2) + ml* rcg* x[9]**2 * sin(x[5] + thgp + pi) - Fyl + ml* g; 
    b[10,0] = Jl * x[13] - Ml; 
    y = np.linalg.inv(a).dot(b);
    return y
x=np.array([1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18])
y=R6dy(x)